#include "mpu6050.h"
#include "mpu6050_port.h"

void MPU6050Init(void)
{
	MPU6050PortWriteReg(MPU6050_REG_PWR_MGMT_1, 0x00);
	MPU6050PortWriteReg(MPU6050_REG_SMPRT_DIV, 0x07);
	MPU6050PortWriteReg(MPU6060_REG_CONFIG, 0x06);
	MPU6050PortWriteReg(MPU6050_REG_ACCEL_CONFIG, 0x01);
	MPU6050PortWriteReg(MPU6050_REG_GYRO_CONFIG, 0x18);
}

uint8_t MPU6050ReadID(void)
{
	uint8_t regVal;
	
	MPU6050ReadOneReg(MPU6050_REG_WHO_AM_I, &regVal);
	
	return regVal;
}

void MPU6050ReadAcceleration(uint16_t *acc)
{
	uint8_t buf[6];
	
	MPU6050PortReadArray(MPU6050_REG_ACCEL_XOUT_H, buf, 6);
	acc[0] = (buf[0] << 8) | buf[1];
	acc[1] = (buf[2] << 8) | buf[3];
	acc[2] = (buf[4] << 8) | buf[5];
}
